#include "color_sensor.h"

#include "peripheral.h"

#include "status.h"

#include "buzzer.h"
color_t color;


static void color_brightness(u8 percent)
{
  u8 data[2];
  data[0] = 0xc1;
  data[1] = percent;
  HAL_UART_Transmit(&uart3_handle, data, 2, 10);
}


static void color_white_balance()
{
  WRITE_REG(USART3->DR, 0xc2);
  delay_ms(1000);
}

//static uint8_t color_get_r(void)
//{
//  
//  WRITE_REG(USART3->DR, 0xd5);
//  	
//  return (uint8_t) READ_REG(USART3->DR);
//	
//	buzzer_on();

//}
static uint8_t color_get_r(void)
{
  status.color_msg = 0;
  
  WRITE_REG(USART3->DR, 0xd5);
  
  while (status.color_msg != 3);
	
  return 100;//(uint8_t) READ_REG(USART3->DR);
	
}

static uint8_t color_get_g(void)
{
  status.color_msg = 0;
  WRITE_REG(USART3->DR, 0xd6);
  
  while (status.color_msg != 3);
  
  return (uint8_t) READ_REG(USART3->DR);
}

static uint8_t color_get_b(void)
{
  status.color_msg = 0;
  WRITE_REG(USART3->DR, 0xd7);
  
  while (status.color_msg != 3);
  
  return (uint8_t) READ_REG(USART3->DR);
}

void color_get(void)
{
  color.r = color_get_r();
  color.g = color_get_g(); 
  color.b = color_get_b();
}


//unsigned char color_cube(void)
//{
//  u8 r,g,b;
//  r = 100;
//  g = 0;
//  b = 0;
//  
//  /* 这个方法不严谨，需要针对比赛进行优化，用算法判断物块颜色 */
//  if (r>g && r>b)
//    buzzer_on();
//  if (b>r && b>g)
//    return 3;
//  else 
//    return 2;
//}
/* 返回路线，1,2,3 */
unsigned char color_cube(void)
{
  u8 r,g,b;
  r = color_get_r();
  g = color_get_g();
  b = color_get_b();
  
  /* 这个方法不严谨，需要针对比赛进行优化，用算法判断物块颜色 */
  if (r>g && r>b)
    return 1;
  if (b>r && b>g)
    return 3;
  else 
    return 2;
}


void color_sensor_init(void)
{

  /* 颜色传感器配置 */
  color_brightness(10);
  
  color_white_balance();

//  //白平衡，确保RGB值均>250
//  while (col_inf.r_val < 250 || col_inf.g_val < 250 || col_inf.b_val < 250)
//  {
//    Get_average_RGB(10);
//    Set_white_balance();
//  }


}
  


void color_sensor_test(void)
{
  color_brightness(1);   /* 亮度设置 */
  
  color_white_balance();   /* 设置白平衡 */
   
  while(1)
  {
    printf("r: %d\n",color_get_r());
    printf("g: %d\n",color_get_g());
    printf("b: %d\n",color_get_b());
    
    printf("==================");
    
    delay_ms(500);  
  }
}

  

